#include "motor_controller.h"

#include <cmath>
#include <rclcpp/rclcpp.hpp>
#include <thread>

#include "can_open_driver.h"

constexpr double PI = M_PI;                     // 使用预定义的 π 值
constexpr double WHEEL_DIAMETER = 0.125;        // 轮子直径 单位:米
constexpr int MOTOR_FEEDBACK_ACCURACY = 10000;  // 电机反馈精度
constexpr double CALCULATE_TURNING_TIME = 1;    // 转向计算时间 单位:秒
constexpr int RUNNING_PERIOD = 10;              // 运行周期 单位:毫秒
constexpr int WAITING_FOR_REBOOT = 2000;        // 运行周期 单位:毫秒
constexpr int WAITING_FIND_ORIGINAL = 5000;     // 运行周期 单位:毫秒
constexpr double SPEED_RATIO = 18.56;           // 减速比 1：18.56
constexpr double GEAR_RATIO = 128.3;            // 齿轮比 1：128.3
constexpr double DECELERATION_RATIO = 0.1;      // 无指令时的减速比
constexpr uint16_t COMMAND_NOT_RECEIVED_TIME =
    300;  // 没收到指令的时间 单位：毫秒

#define M_S_TO_DEC(linear)                                        \
  (linear * 60 / WHEEL_DIAMETER / PI * SPEED_RATIO * 512 / 1875 * \
   MOTOR_FEEDBACK_ACCURACY)
#define DEC_TO_M_S(linear)                                        \
  (linear / 60 * WHEEL_DIAMETER * PI / SPEED_RATIO / 512 * 1875 / \
   MOTOR_FEEDBACK_ACCURACY)
#define RPS_S_TO_DEC(speed) \
  (speed * 65536 / (4000000 / MOTOR_FEEDBACK_ACCURACY))
#define RPM_TO_DEC(velocity) (velocity * 512 / 1875 * MOTOR_FEEDBACK_ACCURACY)
#define RAD_S_TO_DEC(angular)                                              \
  (angular * CALCULATE_TURNING_TIME * MOTOR_FEEDBACK_ACCURACY / (2 * PI) * \
   GEAR_RATIO)
#define DEC_TO_RAD(dec) \
  (dec / MOTOR_FEEDBACK_ACCURACY * (2 * PI) / GEAR_RATIO)

void PowerMotorController::init() {
  m_running = true;
  uint32_t data = static_cast<uint32_t>(RPS_S_TO_DEC(50));
  uint8_t size = 4;
  m_driver->sdoDownload(m_motorId, 0x6083, 0, data, size);
  m_driver->sdoDownload(m_motorId, 0x6084, 0, data, size);
  std::thread(&PowerMotorController::timer, this).detach();
}

void PowerMotorController::powerOnSelfCheck() {
  motorReboot();
  uint32_t data = 0xF;
  uint8_t size = 2;
  m_driver->sdoDownload(m_motorId, 0x6040, 0, data, size);
  data = 0;
  size = 1;
  std::this_thread::sleep_for(std::chrono::milliseconds(200));
  m_driver->sdoUpload(m_motorId, 0x6061, 0, data, size);
  if (static_cast<int8_t>(data) != static_cast<int8_t>(WorkMode::SPEED_MODE)) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
                "driver motor %u work mode err %u", m_motorId, data);
    exit(EXIT_FAILURE);
  }
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "driver motor %u ok", m_motorId);
}

void SteeringMotorController::init() {
  m_running = true;
  uint32_t data = static_cast<uint32_t>(RPM_TO_DEC(1500));
  uint8_t size = 4;
  m_driver->sdoDownload(m_motorId, 0x6081, 0, data, size);
  std::thread(&SteeringMotorController::timer, this).detach();
}

void SteeringMotorController::powerOnSelfCheck() {
  motorReboot();
  uint32_t data = 0xF;
  uint8_t size = 2;
  m_driver->sdoDownload(m_motorId, 0x6040, 0, data, size);
  data = 0;
  size = 1;
  std::this_thread::sleep_for(std::chrono::milliseconds(200));
  m_driver->sdoUpload(m_motorId, 0x6061, 0, data, size);
  if (static_cast<int8_t>(data) ==
      static_cast<int8_t>(WorkMode::POSITION_MODE)) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "steeing motor %u ok", m_motorId);
    return;
  }
  if (static_cast<int8_t>(data) ==
      static_cast<int8_t>(WorkMode::ORIGINAL_MODE)) {
    std::this_thread::sleep_for(
        std::chrono::milliseconds(WAITING_FIND_ORIGINAL));
    data = 0;
    size = 1;
    m_driver->sdoUpload(m_motorId, 0x6061, 0, data, size);
    if (static_cast<int8_t>(data) !=
        static_cast<int8_t>(WorkMode::POSITION_MODE)) {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
                  "steeing motor %u work mode err %u", m_motorId, data);
      exit(EXIT_FAILURE);
    }
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "steeing motor %u ok", m_motorId);
    return;
  }
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "steeing motor %u work mode err %u",
              m_motorId, data);
}

void MotorController::setAbsPosition(double angular) {
  m_commandReceived = true;
  m_commandReceivedTime = std::chrono::steady_clock::now();
  m_angular = angular;
  m_position = static_cast<int32_t>(RAD_S_TO_DEC(m_angular));
}

void MotorController::setAbsPositionFunc() {
  uint32_t data = 0x2F;
  uint8_t size = 2;
  m_driver->sendPDO2(m_motorId, reinterpret_cast<uint8_t*>(&data), size);

  data = static_cast<uint32_t>(m_position);
  size = 4;
  m_driver->sendPDO1(m_motorId, reinterpret_cast<uint8_t*>(&data), size);

  data = 0x3F;
  size = 2;
  m_driver->sendPDO2(m_motorId, reinterpret_cast<uint8_t*>(&data), size);
}

void MotorController::setTargetVelocity(double linear) {
  m_commandReceived = true;
  m_commandReceivedTime = std::chrono::steady_clock::now();
  m_linear = linear;
}

void MotorController::setTargetVelocityFunc() {
  uint32_t data = static_cast<uint32_t>(M_S_TO_DEC(m_linear));
  uint8_t size = 4;
  m_driver->sendPDO1(m_motorId, reinterpret_cast<uint8_t*>(&data), size);
}

double MotorController::getCurrentPosition() const {
  uint32_t data = 0;
  uint8_t size = 4;
  m_driver->sdoUpload(m_motorId, 0x6063, 0, data, size);
  return static_cast<double>(DEC_TO_RAD(static_cast<int32_t>(data)));
}

double MotorController::getCurrentVelocity() const {
  uint32_t data = 0;
  uint8_t size = 4;
  m_driver->sdoUpload(m_motorId, 0x606C, 0, data, size);
  return static_cast<double>(DEC_TO_M_S(static_cast<int32_t>(data)));
}

double MotorController::getLinear() const { return m_currentLinear; }

double MotorController::getAngular() const { return m_currentAngular; }

void SteeringMotorController::updatePosition() {
  setAbsPositionFunc();
  m_currentAngular = getCurrentPosition();
}

void PowerMotorController::updateVelocity() {
  setTargetVelocityFunc();
  m_currentLinear = getCurrentVelocity();
}

void SteeringMotorController::timer() {
  while (m_running) {
    m_driver->sendHeartBeat(m_motorId);
    if (!m_driver->isCanDriverRunning()) {
      return;
    }
    m_commandReceived = false;
    std::this_thread::sleep_for(std::chrono::milliseconds(RUNNING_PERIOD));
    if (!m_commandReceived && m_running) {
      auto now = std::chrono::steady_clock::now();
      if (std::chrono::duration_cast<std::chrono::milliseconds>(
              now - m_commandReceivedTime)
              .count() > COMMAND_NOT_RECEIVED_TIME) {
        stopMotor();
      }
    }
    updatePosition();
  }
}

void PowerMotorController::timer() {
  while (m_running) {
    m_driver->sendHeartBeat(m_motorId);
    if (!m_driver->isCanDriverRunning()) {
      stopMotor();
      setTargetVelocityFunc();
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "stop %u", m_motorId);
      return;
    }
    m_commandReceived = false;
    std::this_thread::sleep_for(std::chrono::milliseconds(RUNNING_PERIOD));
    if (!m_commandReceived && m_running) {
      auto now = std::chrono::steady_clock::now();
      if (std::chrono::duration_cast<std::chrono::milliseconds>(
              now - m_commandReceivedTime)
              .count() > COMMAND_NOT_RECEIVED_TIME) {
        stopMotor();
      }
    }
    updateVelocity();
  }
}

void SteeringMotorController::stopMotor() { m_position = 0; }

void PowerMotorController::stopMotor() { m_linear = 0; }

void MotorController::motorReboot() {
  uint32_t data = 0xAA55;
  uint8_t size = 2;
  m_driver->sdoDownload(m_motorId, 0x2FFF, 0, data, size);
  std::this_thread::sleep_for(std::chrono::milliseconds(WAITING_FOR_REBOOT));
}